FUNCTION_BLOCK LCB_SetAxisData
TITLE = 'LCB_SetAxisData'

// This FB / Data_Block serves to indicate the machine parameters of a master drive. The FB
// prepares the machine parameters which you define with the physical units of
// the machine via parameters FOR the internal measuring system OF the profile generator.
// 
// Versionshistory:
// 1.0 Ersterstellung R.Enders / E.Dittmann  

VERSION : '1.0'
AUTHOR  : 'Lenze'
FAMILY  : 'LCB'
NAME : 'SetAxisD' // ( 8 Zeichen max.)
KNOW_HOW_PROTECT

VAR_INPUT
    rFeedConstant        : REAL  := 360.0;
    rCycleLength         : REAL  := 360.0;
    diGearNumerator      : DINT :=1;
    diGearDenominator    : DINT :=1;
    rReferenceSpeed      : REAL  :=180000.0;
    rReferenceTorque     : REAL   :=1.0;
    sAxisName            : STRING[16] := 'MyAxis';
    diResolution         : DINT := 16;
    xModeUnlimited       : BOOL := True;
    xModeLimited         : BOOL := False;
    xModeModulo          : BOOL :=False;
    
END_VAR

VAR_OUTPUT
    
  END_VAR

VAR
    rInvFeedConstant     : REAL;
    diReferenceSpeed_s   : DINT;
    rEncoderGear         : REAL;
    rInvEncoderGear      : REAL;
    diCycleLength_p      : DINT;
    rUnitsToPos          : REAL;
    rPosToUnits          : REAL;
    rUnitsToSpeed        : REAL;
    rSpeedToUnits        : REAL;
    rTimeUnit            : REAL := 60;
    
    rFeedConstantOld     : REAL  := 0.0;
    rCycleLengthOld      : REAL  := 0.0;
    diGearNumeratorOld   : DINT := 2;
    diGearDenominatorOld : DINT := 2;
    rReferenceSpeedOld   : REAL  := 0.0;
    diResolutionOld      : DINT := 1;
    xModeUnlimitedOld    : BOOL  := False; 
    xModeLimitedOld      : BOOL  := False;
    xModeModuloOld       : BOOL  := False;     
 END_VAR


BEGIN
    IF xModeUnlimited= True THEN xModeLimited:= False; xModeModulo:= False;
        ELSIF xModeLimited= True THEN xModeUnlimited:= False; xModeModulo:= False;
            ELSIF xModeModulo= True THEN xModeUnlimited:= False; xModeLimited:= False;
    END_IF; 
    IF ((rFeedConstantOld <> rFeedConstant) OR
        (diGearNumeratorOld <> diGearNumerator) OR
        (diGearDenominatorOld <> diGearDenominator) OR
        (rCycleLengthOld <> rCycleLength) OR
        (xModeUnlimitedOld <> xModeUnlimited) OR
        (xModeLimitedOld <> xModeLimited) OR
        (xModeModuloOld <> xModeModulo) OR
        (diResolutionOld <> diResolution) OR
        (rReferenceSpeedOld <> rReferenceSpeed)) THEN
    
    
        rInvFeedConstant := 1.0 / rFeedConstant;

    (* Achsdatenberechnung starten *)
    (* Getriebefaktor *)
        rEncoderGear:=DINT_TO_REAL(diGearNumerator) / DINT_TO_REAL(diGearDenominator);
        rInvEncoderGear:= 1.0 / rEncoderGear;
    (* Umrechenfaktor Units -> _p bestimmen *)
        rUnitsToPos := (rInvFeedConstant * rEncoderGear) * (2**diResolution);
    (* Umrechenfaktor _p -> Units bestimmen *)
        rPosToUnits := 1.0 / rUnitsToPos;
    (* Umrechenfaktor Units->_s bestimmen *)
        rUnitsToSpeed := rInvFeedConstant * rEncoderGear;
  //  (* mal 2^32 (eine Umdrehung) *)
  //      rUnitsToSpeed := rUnitsToSpeed * 2**32;
    (* durch Zeiteinheit teilen *)
        rUnitsToSpeed := rUnitsToSpeed * rTimeUnit;
    (* Umrechenfaktor _s -> Units bestimmen *)
        rSpeedToUnits := 1.0 / rUnitsToSpeed;
    (*Referenzgeschwindigkeit bestimmen *)
        diReferenceSpeed_s := REAL_TO_DINT(rReferenceSpeed * rUnitsToSpeed);

        IF xModeModulo THEN
            diCycleLength_p := REAL_TO_DINT(rCycleLength * rUnitsToPos) ;
        ELSE
            diCycleLength_p := 0;
        END_IF;

    // sichern der aktuellen Werte für Änderungsvergleich
        rFeedConstantOld     :=rFeedConstant     ;
        rCycleLengthOld      :=rCycleLength      ;    
        diGearNumeratorOld   :=diGearNumerator   ;
        diGearDenominatorOld :=diGearDenominator ;
        rReferenceSpeedOld   :=rReferenceSpeed   ;
        xModeUnlimitedOld    :=xModeUnlimited    ;
        xModeLimitedOld      :=xModeLimited      ;
        xModeModuloOld       :=xModeModulo       ;
        diResolutionOld      :=diResolution      ;  
           
    END_IF;
              
                                                                                
 END_FUNCTION_BLOCK


